#include "Fixed_Point_Math.h"

/********** LONGITUDE RATIO FOR LATITUDE POSITION *****************/
/* This table gives the relation betwen latitude and the longitude-distance 
relation. The format is 16 bits of integer part and 6 bits of fractional
part. The units are meters per minute. The first position contains the ratio
at latitue = 0 degrees, the last position is at 90 degrees.*/
  
const uint32 u32_GPS_LongRatio[91] = {
0x73D3FB0,	0x73CF76E,	0x73C1EAD,	0x73AB57F,	0x738BBFF,	0x7363255,	
0x73318B3,	0x72F6F58,	0x72B368D,	0x7266EA5,	0x7211801,	0x71B330A,	
0x714C037,	0x70DC009,	0x706330A,	0x6FE19D2,	0x6F57502,	0x6EC4547,	
0x6E28B59,	0x6D847F9,	0x6CD7BF5,	0x6C22823,	0x6B64D66,	0x6A9ECAC,	
0x69D06EA,	0x68F9D22,	0x681B060,	0x67341BA,	0x6645251,	0x654E34E,	
0x644F5E5,	0x6348B54,	0x623A4E4,	0x61243E4,	0x60069B2,	0x5EE17B0,	
0x5DB4F4C,	0x5C811FE,	0x5B46145,	0x5A03EAB,	0x58BABC2,	0x576AA24,	
0x5613B74,	0x54B615F,	0x5351D98,	0x51E71DD,	0x5075FF1,	0x4EFE9A2,	
0x4D810C3,	0x4BFD731,	0x4A73ECF,	0x48E4989,	0x474F950,	0x45B501E,	
0x4414FF3,	0x426FAD6,	0x40C52D5,	0x3F15A04,	0x3D6127E,	0x3BA7E62,	
0x39E9FD8,	0x382790C,	0x3660C2F,	0x3495B7B,	0x32C692A,	0x30F377F,	
0x2F1C8C0,	0x2D41F3A,	0x2B63D3D,	0x298251C,	0x279D932,	0x25B5BD9,	
0x23CAF74,	0x21DD667,	0x1FED319,	0x1DFA7F5,	0x1C0576A,	0x1A0E3E9,	
0x1814FE5,	0x1619DD5,	0x141D033,	0x121E978,	0x101EC21,	0xE1DAAE,	
0xC1B79D,	0xA18572,	0x8146AE,	0x60FDD6,	0x40AD6D,	0x2057FA,	
0x0};
    
/*The next value is the latitude-distance constant ratio. It uses the same
format of last table. The value is 1853.2487 meters by minute*/

const uint32 u32_GPS_LatRatio = 0x73D3FB0;

extern uUN32 uUN32_latitudeController;
extern uUN32 uUN32_longitudeController;
extern uUN32 uUN32_latitude;
extern uUN32 uUN32_longitude;
extern uUN32 uUN32_temporal1;
extern uUN32 uUN32_temporal2;
extern uUN64 uUN64_temporal3;
extern uUN32 uUN32_distanceY;
extern uUN32 uUN32_distanceX;
extern uint16 u16GPS_distance;
extern uint8 u8TableLatitude;

#pragma MESSAGE DISABLE C4000
#pragma MESSAGE DISABLE C4001    
void vfnGPS_DistanceAproximation(void)
{
    uint8 u8_DivisionTimes=0;
    uint8 u8_iteraciones=0;
    
    if(uUN32_latitudeController.l > uUN32_latitude.l)    //uUN32_temporal1 contains the latitude increment
    {
        uUN32_temporal1.l = uUN32_latitudeController.l - uUN32_latitude.l;
    }
    else
    {
        uUN32_temporal1.l = uUN32_latitude.l - uUN32_latitudeController.l;
    }

    uUN32_temporal2.l = u32_GPS_LatRatio;     //The latitude ratio is loaded into uUN32_temporal2

    mMul32x32to32Q16(uUN32_temporal1, uUN32_temporal2, uUN32_distanceY, uUN64_temporal3);


    if(uUN32_longitudeController.l > uUN32_longitude.l)    //uUN32_temporal1 contains the latitude increment
    {
        uUN32_temporal1.l = uUN32_longitudeController.l - uUN32_longitude.l;
    }
    else
    {
        uUN32_temporal1.l = uUN32_longitude.l - uUN32_longitudeController.l;
    }

    //It is necesary to know the latitude value with precision of one degree. This data should be 
    //calculated during parsing process

    uUN32_temporal2.l=u32_GPS_LongRatio[u8TableLatitude];
                                                   //The longitude ratio is loaded into uUN32_temporal2

    mMul32x32to32Q16(uUN32_temporal1,uUN32_temporal2,uUN32_distanceX,uUN64_temporal3);
    
    while(uUN32_distanceX.shorts.s1 > 115 | uUN32_distanceY.shorts.s1 > 115 )
    {
        uUN32_distanceX.shorts.s1 = uUN32_distanceX.shorts.s1>>1;
        uUN32_distanceY.shorts.s1 = uUN32_distanceY.shorts.s1>>1;
        u8_DivisionTimes++;
    }
    //Xold -> uUN32_temporal2.shorts.s1
    //Xnew -> uUN32_temporal2.shorts.s0
    
    if(uUN32_distanceX.bytes.b2 > uUN32_distanceY.bytes.b2)
    {
        uUN32_temporal2.shorts.s1 = uUN32_distanceX.bytes.b2;
    }
    else
    {
        uUN32_temporal2.shorts.s1 = uUN32_distanceY.bytes.b2;
    }
    
    uUN32_temporal1.shorts.s0 = uUN32_distanceX.bytes.b2 * uUN32_distanceX.bytes.b2;
    uUN32_temporal1.shorts.s1 = uUN32_distanceY.bytes.b2 * uUN32_distanceY.bytes.b2;
    
    uUN32_temporal1.shorts.s0 += uUN32_temporal1.shorts.s1;
    
    
    while(1)
    {
        uUN32_temporal2.shorts.s0 = uUN32_temporal1.shorts.s0 / uUN32_temporal2.shorts.s1;
        uUN32_temporal2.shorts.s0 += uUN32_temporal2.shorts.s1;
        uUN32_temporal2.shorts.s0 = uUN32_temporal2.shorts.s0>>1;
        
        if(u8_iteraciones == 2)
            break;
        else
            u8_iteraciones++;
        
        uUN32_temporal2.shorts.s1 = uUN32_temporal2.shorts.s0;
    }
    
    uUN32_temporal2.shorts.s0 = uUN32_temporal2.shorts.s0 << u8_DivisionTimes;
    
    u16GPS_distance = uUN32_temporal2.shorts.s0;
}

